//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
//added HIL
/*
 * vx_tele.ino
 *
 * Global variables accessed:
 *  - g_gps --- for gps data
 *  - ahrs --- for roll pitch yaw
 *  - compass.heading --- magnetometer heading
 *
 * Vexredus variables
 *  - vx_g_stab_pidlist_fbw --- for PID values (fly-by-wire)
 */
 
#include <util/crc16.h>

/* "Internal" variables */
String vx_sendBuffer; // Send buffer
String vx_recvBuffer; // Receive buffer
char vx_recv_c; // Receive char
bool vx_recv_buffering = false;// Buffering logic
String vx_recvMsgHeader;
String vx_crc_s;

/* Public functions */

void vx_teleCheckTelecommands() {
  // Read entire serial buffer
#if VX_TELECOMMAND_PORT == VX_SERIAL
  while(Serial.available() > 0) {
    vx_recv_c = Serial.read();
#elif VX_TELECOMMAND_PORT == VX_SERIAL3
  while(Serial3.available() > 0) {
    vx_recv_c = Serial3.read();
#endif

    // Check for new message header ($) -> start buffering
    if(vx_recv_c == '$') {
      vx_recv_buffering = true; // Turn on buffering
      vx_recvBuffer = ""; // Reset receiving buffer
    }
    
    // Check if current mode is set to buffer (incoming message) -> buffer
    if(vx_recv_buffering) {
      vx_recvBuffer.concat(vx_recv_c);
      
      // Check for end of message (\n) -> Reset buffering & Parse message
      if(vx_recv_c == '\n') {
        vx_recv_buffering = false; // Turn off buffering
        
        // Message accepted (but crc not checked)
        vx_g_tele_last_received_time = millis();
        
        // Extract message header
        vx_recvMsgHeader = vx_recvBuffer.substring(1,2);
        
        // Get message type
        if(vx_recvMsgHeader.equals("R")) { // Parachute control -> don't checksum
          vx_teleParseParachuteControl();
        }
        else {
          // Checksum
          String vx_recvBuffer_nochecksum = vx_recvBuffer.substring(0, (vx_recvBuffer.length()-4));
          vx_teleChecksum(vx_recvBuffer_nochecksum);
          //Serial.println("DEBUG: calculated checksum for vx_recvBuffer = " + vx_crc_s);
          
          if(vx_crc_s != vx_recvBuffer.substring((vx_recvBuffer.length()-3),(vx_recvBuffer.length()-1))) {
            vx_teleSendWarning(VX_TELE_WARNING_CRCERR);
          }
          else {  
            if(vx_recvMsgHeader.equals("B")) {
              vx_teleParseSetHomeBase();              
            }
            else if(vx_recvMsgHeader.equals("M")) {
              vx_teleParseSetMode();
            }
            else if(vx_recvMsgHeader.equals("W")) {
              vx_teleParseSetWaypoint();
            }
            else if(vx_recvMsgHeader.equals("J")) {
              vx_teleParseSetPID();
            }
            else if(vx_recvMsgHeader.equals("G")) {
              vx_teleParseGetPID();
            }
            
            else if(vx_recvMsgHeader.equals("D")) {
              vx_teleParseSetDescent();
            }
            else if(vx_recvMsgHeader.equals("E")) {
              vx_teleParseSetUseDescentNick();
            }
            else if(vx_recvMsgHeader.equals("F")) {
              vx_teleParseSetUseDescentHeading();
            }
            
            else if(vx_recvMsgHeader.equals("C")) {
              vx_teleParseSetCountdownClock();
            }
            
            else if(vx_recvMsgHeader.equals("L")) {
              vx_teleParseSetGsFbwRoll();
            }
            else if(vx_recvMsgHeader.equals("U")) {
              vx_teleParseSetGsFbwNick();
            }
            
            else if(vx_recvMsgHeader.equals("S")) {
              vx_teleParseRequestScalers();
            }
            else if(vx_recvMsgHeader.equals("V")) {
              vx_teleParseWriteScaler();
            }
            
          }
        }
      }
    }
  }
}

// Calculate checksum, sets String vx_crc_s with calculated value
void vx_teleChecksum(String msg) {
  uint8_t crc = 0;
  uint8_t i;
  
  // Calculate checksum
  for(i=0; i<msg.length(); i++) {
    crc = _crc_ibutton_update(crc, (uint8_t)msg[i]);
  }
  
  // Convert to a nice string
  //vx_crc_s = "";
  if(crc < 16) {
    //vx_crc_s.concat('0');
    vx_crc_s = "0";
  }
  else {
    vx_crc_s = "";
  }
  
  vx_crc_s.concat(String(crc, HEX));
  vx_crc_s.toUpperCase();
}

void vx_teleSerialOutCRC() {
  // Sends current message WITH CRC
  vx_teleChecksum(vx_sendBuffer); // calculate crc 
  
  vx_sendBuffer.concat('*'); // checksum separator
  vx_sendBuffer.concat(vx_crc_s); // checksum
  vx_sendBuffer.concat('\n'); // End of Message

#if VX_TELEMETRY_PORT == VX_SERIAL
  Serial.print(vx_sendBuffer);
#elif VX_TELEMETRY_PORT == VX_SERIAL3
  Serial3.print(vx_sendBuffer);
#elif VX_TELEMETRY_PORT == VX_SERIAL_BOTH
  Serial.print(vx_sendBuffer);
  Serial3.print(vx_sendBuffer);
#endif

  vx_sendBuffer = ""; // Reset vx_sendBuffer
}

void vx_teleSerialOutNOCRC() {
  // Sends current message WITHOUT CRC
  vx_sendBuffer.concat('\n'); // End of Message
  
#if VX_TELEMETRY_PORT == VX_SERIAL
  Serial.print(vx_sendBuffer);
#elif VX_TELEMETRY_PORT == VX_SERIAL3
  Serial3.print(vx_sendBuffer);
#elif VX_TELEMETRY_PORT == VX_SERIAL_BOTH
  Serial.print(vx_sendBuffer);
  Serial3.print(vx_sendBuffer);
#endif
  
  vx_sendBuffer = ""; // Reset vx_sendBuffer
}

void vx_teleConcatFloatAndSep(float f, bool sep=true, int prec=4) {
  char dtostrf_buffer[15];
  String dtostrf_s;
  dtostrf(f, 10, prec, dtostrf_buffer); // width = 10
  dtostrf_s = String(dtostrf_buffer);
  dtostrf_s.replace(" ", ""); // Remove space at start
  while(dtostrf_s.endsWith("0")) { // Remove trailing zeroes
    dtostrf_s = dtostrf_s.substring(0,dtostrf_s.length()-1);
  }
  if(dtostrf_s.endsWith(".")) { // Remove trailing dot
    dtostrf_s = dtostrf_s.substring(0,dtostrf_s.length()-1);
  }
  vx_sendBuffer.concat(dtostrf_s);
  
  // Alternatively add separator
  if(sep) {
    vx_sendBuffer.concat(", ");
  }
}

// Concat a pid list to the sending buffer
void vx_teleConcatPIDList(pidlist_t pidl) {
  
  vx_teleConcatFloatAndSep(pidl.a_1);
  vx_teleConcatFloatAndSep(pidl.a_2);
  vx_teleConcatFloatAndSep(pidl.a_3);
  vx_teleConcatFloatAndSep(pidl.a_4);
  vx_teleConcatFloatAndSep(pidl.a_5);
  
  vx_teleConcatFloatAndSep(pidl.b_1);
  vx_teleConcatFloatAndSep(pidl.b_2);
  vx_teleConcatFloatAndSep(pidl.b_3);
  vx_teleConcatFloatAndSep(pidl.b_4);
  vx_teleConcatFloatAndSep(pidl.b_5);
  vx_teleConcatFloatAndSep(pidl.b_6, false); // Last value: no separator
  
   
  //DEBUGGING
  /*Serial.print("***DEBUG: pidl: ");
  Serial.print(pidl.a_1,2);
  Serial.print(" , ");
  Serial.print(pidl.a_2,2);
  Serial.print(" , ");
  Serial.print(pidl.a_3,2);
  Serial.print(" , ");
  Serial.print(pidl.a_4,2);
  Serial.print(" , ");
  Serial.print(pidl.a_5,2);
  Serial.println(" , ");
  Serial.print(pidl.b_1,2);
  Serial.print(" , ");
  Serial.print(pidl.b_2,2);
  Serial.print(" , ");
  Serial.print(pidl.b_3,2);
  Serial.print(" , ");
  Serial.print(pidl.b_4,2);
  Serial.print(" , ");
  Serial.print(pidl.b_5,2);
  Serial.print(" , ");
  Serial.println(pidl.b_6,2);*/
}

/*** Telemetry functions ***/
void vx_teleSendCurrentPosition() {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$P"; 

  //vx_sendBuffer.concat(String(g_gps->latitude, HEX)); // LONG - Var 1: Latitude
  vx_sendBuffer.concat(String(vx_g_current_pos.lat, HEX)); // LONG - Var 1: Latitude (sanitized)
  vx_sendBuffer.concat(','); // Seperator 
  //vx_sendBuffer.concat(String(g_gps->longitude, HEX)); // LONG - Var 2: Longitude
  vx_sendBuffer.concat(String(vx_g_current_pos.lon, HEX)); // LONG - Var 2: Longitude (sanitized)
  vx_sendBuffer.concat(','); // Seperator
  //vx_sendBuffer.concat(String(g_gps->altitude, HEX)); // LONG - Var 3: Altitude
  vx_sendBuffer.concat(String(vx_g_current_pos.alt, HEX)); // LONG - Var 3: Altitude (sanitized)
  vx_sendBuffer.concat(','); // Seperator
  
  #if VX_SIM == VX_SIM_NONE
  vx_sendBuffer.concat(String(g_gps->num_sats));// UINT8_T - Var 4: Number of satellites (GPS validity)
  vx_sendBuffer.concat(','); // Seperator
  vx_sendBuffer.concat(String(g_gps->status()));
  #endif
  
  #if VX_SIM == VX_SIM_HIL
  vx_sendBuffer.concat("666");// UINT8_T - Var 4: Number of satellites (GPS validity)
  vx_sendBuffer.concat(','); // Seperator
  vx_sendBuffer.concat("2");
  #endif
  
  vx_sendBuffer.concat(','); // Seperator
  vx_sendBuffer.concat(String(vx_g_flightphase));
  
  
  // Send message
  vx_teleSerialOutCRC();
  
  //Serial.println(g_gps->status());
}

void vx_teleSendCurrentAttitude() {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$A";
  
  #if VX_SIM == VX_SIM_NONE
  vx_sendBuffer.concat(String((int)(ToDeg(ahrs.roll)*100))); //, HEX)); // &FLOAT - Var 1: roll
  vx_sendBuffer.concat(','); // Seperator 
  vx_sendBuffer.concat(String((int)(ToDeg(ahrs.pitch)*100))); //, HEX)); // &FLOAT - Var 2: pitch
  #endif
  #if VX_SIM == VX_SIM_HIL
  vx_sendBuffer.concat(String((int)(ToDeg(vx_g_hil_roll)*100))); //, HEX)); // &FLOAT - Var 1: roll
  vx_sendBuffer.concat(','); // Seperator 
  vx_sendBuffer.concat(String((int)(ToDeg(vx_g_hil_nick)*100))); //, HEX)); // &FLOAT - Var 2: pitch
  #endif
  vx_sendBuffer.concat(','); // Seperator 
  vx_sendBuffer.concat(String((int)(ToDeg(vx_g_target_heading)*100))); //, HEX)); // &FLOAT - Var 3: yaw
//#if VX_STAB_PID_TESTING != 1
  vx_sendBuffer.concat(','); // Seperator
  //vx_sendBuffer.concat(String(g_gps->ground_course)); //, HEX)); // LONG - Var 4: gps heading (ground course)
  vx_sendBuffer.concat(String(vx_g_current_ground_course)); //, HEX)); // LONG - Var 4: gps heading (ground course) (sanitized)
  vx_sendBuffer.concat(','); // Seperator 
  //vx_sendBuffer.concat(String(g_gps->ground_speed)); //, HEX)); // LONG - Var 5: gps ground speed
  vx_sendBuffer.concat(String(vx_g_current_ground_speed)); //, HEX)); // LONG - Var 5: gps ground speed (sanitized)
  vx_sendBuffer.concat(','); // Seperator 
  vx_sendBuffer.concat(String( vx_g_wpdistance, HEX));
//#endif
  
   
  // Send Message
  vx_teleSerialOutCRC();
}

void vx_teleSendHousekeeping() {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$H";

  vx_sendBuffer.concat(String((unsigned long)(millis()/1000))); // Millis since restart
  vx_sendBuffer.concat(','); // Separator
  vx_sendBuffer.concat(String(vx_g_para_countdown_clock)); // Parachute countdown clock
  vx_sendBuffer.concat(','); // Separator
  
  //long homebase_dist = vx_calculate_wp_distance(vx_g_current_pos.lat, vx_g_current_pos.lon, vx_g_home.lat, vx_g_home.lon);
  long homebase_dist = vx_calculate_wp_distance(vx_g_current_pos.lat, vx_g_current_pos.lon, vx_g_center_circle.lat, vx_g_center_circle.lon);
  vx_sendBuffer.concat(String(homebase_dist)); // in meters. Current calculated distance to homepoint
  vx_sendBuffer.concat(','); // Separator
  
  if(vx_g_para_ascent == true) vx_sendBuffer.concat(1); // Current ascent mode
  else vx_sendBuffer.concat(0);
  vx_sendBuffer.concat(','); // Separator
  
  vx_sendBuffer.concat(String((millis() - vx_g_tele_last_received_time)/1000)); // in seconds, last received tele time

  
  // Send Message
  vx_teleSerialOutCRC();
}

void vx_teleSendWarning(int warning) {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$W";
  
  // Variable 1: warning message
  
  // PARACHUTE warning messages
  if(warning == VX_TELE_WARNING_PARACHUTE__BOUNDARYBOX) {
    vx_sendBuffer.concat("PARACHUTE_BOUNDARY");  // Releasing parachute in 10(??) seconds, due to boundary box
  }
  else if(warning == VX_TELE_WARNING_PARACHUTE__NOGPSFIX) {
    vx_sendBuffer.concat("PARACHUTE_NOGPSFIX");  // Releasing parachute in 10(??) seconds, due to no ground station contact && no gps fix 
  }
  else if(warning == VX_TELE_WARNING_PARACHUTE__LOWALTITUDE) {
    vx_sendBuffer.concat("PARACHUTE_LOWALT");  // Releasing parachute in 10(??) seconds, due to no ground station contact && too low alt
  }
  else if(warning == VX_TELE_WARNING_RELEASED) {
    vx_sendBuffer.concat("RELEASED"); // Released parachute
  }
  else if(warning == VX_TELE_WARNING_CANCELED) {
    vx_sendBuffer.concat("CANCELED"); // Manual cancel of parachute
  }
  
  // PARACHUTE cancel messages
  else if(warning == VX_TELE_WARNING_CANCELED__NOGPSFIX_GPSFIX) {
    vx_sendBuffer.concat("C_NOGPSFIX_GPSFIX"); 
  }
  else if(warning == VX_TELE_WARNING_CANCELED__NOGPSFIX_GS) {
    vx_sendBuffer.concat("C_NOGPSFIX_GS"); 
  }
  else if(warning == VX_TELE_WARNING_CANCELED__NOGPSFIX_RC) {
    vx_sendBuffer.concat("C_NOGPSFIX_RC"); 
  }
  else if(warning == VX_TELE_WARNING_CANCELED__BOUNDARYBOX) {
    vx_sendBuffer.concat("C_BOUNDARYBOX"); 
  }
  else if(warning == VX_TELE_WARNING_CANCELED__LOWALTITUDE_RC) {
    vx_sendBuffer.concat("C_LOWALTITUDE_RC"); 
  }
  else if(warning == VX_TELE_WARNING_CANCELED__LOWALTITUDE_GS) {
    vx_sendBuffer.concat("C_LOWALTITUDE_GS"); 
  }
  
  // LAUNCH warning messages
  else if(warning == VX_TELE_WARNING_UAVLAUNCH__MANUAL) {
    vx_sendBuffer.concat("LAUNCH_MANUAL");
  }
  else if(warning == VX_TELE_WARNING_UAVLAUNCH__AUTO_BOX) {
    vx_sendBuffer.concat("LAUNCH_ABOX");
  }
  else if(warning == VX_TELE_WARNING_UAVLAUNCH__AUTO_DROP) {
    vx_sendBuffer.concat("LAUNCH_ADROP");
  }
  
  // SMOKER messages
  else if(warning == VX_TELE_WARNING_SMOKER_1) {
    vx_sendBuffer.concat("SMOKER_1_ON");
  }
  else if(warning == VX_TELE_WARNING_SMOKER_2) {
    vx_sendBuffer.concat("SMOKER_2_ON");
  }
  else if(warning == VX_TELE_WARNING_SMOKER_OFF) {
    vx_sendBuffer.concat("SMOKER_IS_OFF");
  }
  
  // TELEMETRY errors
  else if(warning == VX_TELE_WARNING_CRCERR) {
    vx_sendBuffer.concat("CRCERR"); // Checksum error on last telecommand
  }
  else {
    vx_sendBuffer.concat("ERROR");
  }

  
  // Send Message WITHOUT CRC 
  vx_teleSerialOutNOCRC();
}

void vx_teleSendScalers() {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$T";
  
  vx_teleConcatFloatAndSep(vx_scaler_gpsspeed);
  vx_teleConcatFloatAndSep(vx_g_scaler_tele);
  vx_teleConcatFloatAndSep(vx_g_speed_scaler);

  vx_teleSerialOutCRC();
}

// Message structure: $N32,12,0,1*FF
// WHERE: 
//  32: fixed_flightpathangle
//  12: fixed_target_heading
//  0: use_fixed_flightpathangle
//  1: use_fixed_targetheading
void vx_teleSendDescent() {
  vx_sendBuffer = "$N";
  
  // Prepare variables
  int fixed_flightpathangle = RadtoDeg(vx_g_fixed_flightpathangle);
  int fixed_target_heading = RadtoDeg(vx_g_fixed_target_heading);
  
  int use_fixed_flightpathangle;
  int use_fixed_target_heading;
  
  if(vx_g_use_fixed_flightpathangle == 1) use_fixed_flightpathangle = 1;
  else use_fixed_flightpathangle = 0;
  
  if(vx_g_use_fixed_target_heading == 1) use_fixed_target_heading = 1;
  else use_fixed_target_heading = 0;
  
  // Prepare message
  vx_sendBuffer.concat(fixed_flightpathangle); 
  vx_sendBuffer.concat(',');
  vx_sendBuffer.concat(fixed_target_heading); 
  vx_sendBuffer.concat(',');
  vx_sendBuffer.concat(use_fixed_flightpathangle);
  vx_sendBuffer.concat(',');
  vx_sendBuffer.concat(use_fixed_target_heading);
  
  vx_teleSerialOutCRC();
}

void vx_teleSendPID(int pidlist_num) {
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$I";
  vx_sendBuffer.concat(String(pidlist_num));
  vx_sendBuffer.concat(',');
  
  switch(pidlist_num) {
    case VX_TELE_PIDLIST_FBW:
      vx_teleConcatPIDList(vx_g_stab_pidlist_fbw);
      break;
    case VX_TELE_PIDLIST_NAV1_STAB:
      vx_teleConcatPIDList(vx_g_stab_pidlist_nav1);
      break;
    case VX_TELE_PIDLIST_NAV1_NAV:
      vx_teleConcatPIDList(vx_g_nav_pidlist_nav1);
      break;       
    /*case VX_TELE_PIDLIST_NAV2_STAB:
      vx_teleConcatPIDList(vx_g_stab_pidlist_nav2);
      break;
    case VX_TELE_PIDLIST_NAV2_NAV:
      vx_teleConcatPIDList(vx_g_nav_pidlist_nav2);
      break;
    case VX_TELE_PIDLIST_DIVE_STAB:
      vx_teleConcatPIDList(vx_g_stab_pidlist_dive);
      break;
    case VX_TELE_PIDLIST_DIVE_NAV:
      vx_teleConcatPIDList(vx_g_nav_pidlist_dive);
      break;
    case VX_TELE_PIDLIST_TRANSITION_STAB:
      vx_teleConcatPIDList(vx_g_stab_pidlist_transition);
      break;
    case VX_TELE_PIDLIST_TRANSITION_NAV:
      vx_teleConcatPIDList(vx_g_nav_pidlist_transition);
      break;*/
    default:
      vx_sendBuffer.concat("ERROR_NO_PIDLIST_NUM");
      break;
  }
  
  // Send Message
  vx_teleSerialOutCRC();
}

/*** Telecommand functions ***/
void vx_teleParseParachuteControl() {
  String msg = vx_recvBuffer.substring(2, vx_recvBuffer.length()-1);
  
  // Parachute
  if(msg.equals("RELEASE")) { // RELEASE THE PARACHUTE
    vx_para_manual_release();
  }
  else if(msg.equals("CANCEL")) { // CANCEL CURRENT PARACHUTE RELEASE & RESET PARACHUTE TO REBOOT VALUES
    vx_para_manual_cancel();
  }
  else if(msg.equals("DELAY30")) { // DELAY PARACHUTE RELEASE 30 SECONDS
    vx_para_manual_delay(30);
  }
  else if(msg.equals("DELAY60")) { // DELAY PARACHUTE RELEASE 60 SECONDS
    vx_para_manual_delay(60);
  }
  else if(msg.equals("DELAY120")) { // DELAY PARACHUTE RELEASE 120 SECONDS
    vx_para_manual_delay(120);
  }
  /*else if(msg.equals("RESET")) { // RESET PARACHUTE TO REBOOT VALUES
    vx_para_manual_reset();
  }*/
  
  else if(msg.equals("ASCENTON")) {
    vx_para_manual_ascent_on();
    vx_g_launch_servo = false;
  }
  else if(msg.equals("ASCENTOFF")) {
    vx_para_manual_ascent_off();
  }
  
  // UAV launch
  else if(msg.equals("LAUNCH")) {
    vx_launch_manual(); // Also does ascent off
  }
  
  // SMOKER functions
  else if(msg.equals("SMOKER111")) {
    vx_g_smoker_state = 1;
  }
  else if(msg.equals("SMOKER222")) {
    vx_g_smoker_state = 2;
  }
  else if(msg.equals("SMOKEROFF")) {
    vx_g_smoker_state = 0;
  }
}

void vx_teleParseSetMode() {
  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;

  vx_g_flightphase = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  //Serial.print("DEBUG: parseSetMode() called and set to: " + vx_g_flightphase);
  //Serial.println(vx_g_flightphase);
    
}


//##########################################################################################################################################
void vx_teleParseSetWaypoint() {
  //Serial.println("DEBUG: parseSetWaypoint() called");

  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;

  vx_g_wp_guided.lat = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  vx_g_wp_guided.lon = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  vx_g_wp_guided.alt = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
}

// $D40,33*FF -- where 40 is the pitch to set, 33 is target heading
void vx_teleParseSetDescent() {
  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;
  
  // Prepare variables from message
  float fixed_flightpathangle = (float)toRad*vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  float fixed_target_heading = (float)toRad*vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);

  // Sanitize variables
  if(-HALF_PI <= fixed_flightpathangle && fixed_flightpathangle <= HALF_PI) {
    vx_g_fixed_flightpathangle = fixed_flightpathangle;
  } 
  if(0 <= fixed_target_heading && fixed_target_heading < TWO_PI) {
    vx_g_fixed_target_heading = fixed_target_heading;
  } else if(fixed_target_heading == TWO_PI) {
    vx_g_fixed_target_heading = 0;
  }
  
  /*Serial.print("DEBUG: fixed flightpathangle: ");
  Serial.println(vx_g_fixed_flightpathangle, 4); 
  Serial.print("DEBUG: fixed target heading: ");
  Serial.println(vx_g_fixed_target_heading, 4);*/
  
  vx_teleSendDescent();
}

// $E0*FF -- where 1 = YES , 0 = NO
void vx_teleParseSetUseDescentNick() {
  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;
  
  int use_fixed_flightpathangle = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  
  if(use_fixed_flightpathangle == 1) vx_g_use_fixed_flightpathangle = true;
  else vx_g_use_fixed_flightpathangle = false;
  
  vx_teleSendDescent();
}

// $F1*FF -- where 1 = YES , 0 = NO
void vx_teleParseSetUseDescentHeading() {
  int current_pos = 2;
  int max_var_length = 10;
  int next_var = 0;
  int next_var_pos = 0;
  
  int use_fixed_target_heading = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  
  if(use_fixed_target_heading == 1) vx_g_use_fixed_target_heading = true;
  else vx_g_use_fixed_target_heading = false;
  
  vx_g_use_fixed_target_heading_timer = millis();
  
  vx_teleSendDescent();
}


void vx_teleParseSetPID_list(pidlist_t *pidl, int pidlist_num, int next_var_pos, int current_pos, int max_var_length) {
  
  pidl->a_1 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->a_2 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->a_3 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->a_4 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->a_5 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_1 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_2 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_3 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_4 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_5 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  pidl->b_6 = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  
  // Send confirmation that values have been set
  vx_teleSendPID(pidlist_num);
}

void vx_teleParseSetPID() {
  int current_pos = 2;
  int max_var_length = 9;
  int next_var = 0;
  int next_var_pos = 0;
  
  int pidlist_num = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  current_pos = 1 + vx_recvBuffer.indexOf(',', next_var_pos);
  
  switch(pidlist_num) {
    case VX_TELE_PIDLIST_FBW:
      vx_teleParseSetPID_list(&vx_g_stab_pidlist_fbw, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_NAV1_STAB:
      vx_teleParseSetPID_list(&vx_g_stab_pidlist_nav1, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_NAV1_NAV:
      vx_teleParseSetPID_list(&vx_g_nav_pidlist_nav1, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    /*case VX_TELE_PIDLIST_NAV2_STAB:
      vx_teleParseSetPID_list(&vx_g_stab_pidlist_nav2, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_NAV2_NAV:
      vx_teleParseSetPID_list(&vx_g_nav_pidlist_nav2, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_DIVE_STAB:
      vx_teleParseSetPID_list(&vx_g_stab_pidlist_dive, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_DIVE_NAV:
      vx_teleParseSetPID_list(&vx_g_nav_pidlist_dive, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_TRANSITION_STAB:
      vx_teleParseSetPID_list(&vx_g_stab_pidlist_transition, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;
    case VX_TELE_PIDLIST_TRANSITION_NAV:
      vx_teleParseSetPID_list(&vx_g_nav_pidlist_transition, pidlist_num, next_var_pos, current_pos, max_var_length);
      break;*/
    default:
      vx_sendBuffer = "$WERR_WRONG_PID_NUM";
      vx_teleSerialOutNOCRC();
      break;
  }
}

void vx_teleParseGetPID() {
  vx_teleSendPID(vx_teleParseGetNextInt(2, 1, NULL));
}


// $C23*FF -- 23 countdown clock to 23 seconds
void vx_teleParseSetCountdownClock() {
  int current_pos = 2;
  int max_var_length = 9;
  int next_var = 0;
  int next_var_pos = 0;
  
  long countdown_clock = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  
  // Set global parachute count down clock, sanitize first
  if(countdown_clock > 0) {
    vx_g_para_countdown_clock = countdown_clock;
  }
}


// $L-20*FF -- GS FBW bank 20 deg to left
void vx_teleParseSetGsFbwRoll() {
  //Serial.println("DEBUG: vx_teleParseSetGsFbwRoll() called.");
  int current_pos = 2;
  int max_var_length = 9;
  int next_var = 0;
  int next_var_pos = 0;
  
  // Save value in temporary variable
  int gsfbw_roll = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  
  // Sanity checks
  if((gsfbw_roll > -50) && (gsfbw_roll < 50)) {
    // Update global variable
    vx_g_gsfbw_roll = gsfbw_roll;
  }
  
  //Serial.print("DEBUG: vx_teleParseSetGsFbwRoll(): vx_g_gsfbw_roll: ");
  //Serial.println(vx_g_gsfbw_roll);
}

// $U-20*FF -- GS FBW nick 20 deg down
void vx_teleParseSetGsFbwNick() {
  //Serial.println("DEBUG: vx_teleParseSetGsFbwNick() called.");
  int current_pos = 2;
  int max_var_length = 9;
  int next_var = 0;
  int next_var_pos = 0;
  
  // Save value in temporary variable
  int gsfbw_nick = vx_teleParseGetNextInt(current_pos, max_var_length, &next_var_pos);
  
  // Sanity checks
  if((gsfbw_nick > -50) && (gsfbw_nick < 50)) {
    // Update global variable
    vx_g_gsfbw_nick = gsfbw_nick;
  }
  
  //Serial.print("DEBUG: vx_teleParseSetGsFbwNick(): vx_g_gsfbw_nick: ");
  //Serial.println(vx_g_gsfbw_nick);
}




// $BLAT,LON,ALT*FF
void vx_teleParseSetHomeBase() {
  vx_set_home();
  // Clear buffer & Set Start of Message & Header
  vx_sendBuffer = "$B";
  vx_sendBuffer.concat(vx_g_home.lat);
  vx_sendBuffer.concat(',');
  vx_sendBuffer.concat(vx_g_home.lon);
  vx_sendBuffer.concat(',');
  vx_sendBuffer.concat(vx_g_home.alt);
  
  vx_teleSerialOutNOCRC();
  
}

// $S*FF -- request scalers
void vx_teleParseRequestScalers() {
  vx_teleSendScalers();
}

// $V1.0*FF -- write tele scaler
void vx_teleParseWriteScaler() {
  int current_pos = 2;
  int max_var_length = 9;
  int next_var = 0;
  int next_var_pos = 0;
  
  // Save value in temporary variable
  float scaler_tele = vx_teleParseGetNextFloat(current_pos, max_var_length, &next_var_pos);
  
  // Sanity checks
  if((scaler_tele > -10.0) && (scaler_tele < 10.0)) {
    // Update global variable
    vx_g_scaler_tele = scaler_tele;
  }
  
  vx_teleSendScalers();
}

// *********************************** INTERNAL DATA FUNCTIONS *********************************
int vx_teleParseGetNextInt(int start_pos, int max_length, int *next_var_pos) { 
  char c = vx_recvBuffer[start_pos];
  int i_valbuf = 0;
  char c_valbuf[24];
  
  // Get buffer, stop on separators or when exceeding value max (character) length
  while(i_valbuf < max_length && c != '\n' && c != ',' && c != '*' && c != '\0') {
    c_valbuf[i_valbuf] = c;
    i_valbuf++;
    c = vx_recvBuffer[start_pos + i_valbuf];
  }
  
  // Save the last position
  if(next_var_pos != NULL) {
    *next_var_pos = start_pos + i_valbuf;
  }
  
  // Terminate buffer
  c_valbuf[i_valbuf] = '\0';

  // Convert to int
  return atoi(c_valbuf);
}

float vx_teleParseGetNextFloat(int start_pos, int max_length, int *next_var_pos) {
  char c = vx_recvBuffer[start_pos];
  int i_valbuf = 0;
  char c_valbuf[24];
  
  // Get buffer, stop on separators or when exceeding value max (character) length
  while(i_valbuf < max_length && c != '\n' && c != ',' && c != '*' && c != '\0') {
    c_valbuf[i_valbuf] = c;
    i_valbuf++;
    c = vx_recvBuffer[start_pos + i_valbuf];
  }
  
  // Save the last position
  if(next_var_pos != NULL) {
    *next_var_pos = start_pos + i_valbuf;
  }
  
  // Terminate buffer
  c_valbuf[i_valbuf] = '\0';
  
  /*Serial.print("***DEBUG: vx_...NextFloat(): c_valbuf = "); 
  Serial.print(c_valbuf);
  Serial.print(", atof(c_valbuf) = ");
  Serial.println(atof(c_valbuf),4);*/
  
  // Convert to int
  return atof(c_valbuf);
}

